#ifndef FEEDFORWARD_CONTROLLER_H
#define FEEDFORWARD_CONTROLLER_H

#include "Controller.h"
#include "StateEstimator.h"
#include <Robotics/Wrench.h>

/** @brief A class that adds a feedforward torque to the basic
 * control
 */
class FeedforwardController : public RobotController
{
 public:
  FeedforwardController(Robot& robot,RobotController* base=NULL);
  virtual ~FeedforwardController() {}
  virtual void Update(Real dt);
  virtual void Reset();
  virtual bool ReadState(File& f);
  virtual bool WriteState(File& f) const;

  void SolveTorques(Vector& torques,Real dt);
  void ZeroForces();
  void AddForce(int link,const Vector3& f,const Vector3& worldpt);

  RobotController* base;
  RobotStateEstimator* stateEstimator;
  bool enableGravityCompensation,enableFeedforwardAcceleration;
  Vector3 gravity;
  //external forces and moments about the link origin
  vector<Wrench> wrenches;
};


#endif
